Constant/Linear Time Simultaneous Localization and Mapping for Dense Maps

The challenge of building a map of an unknown environment while simultaneously localizing accurately within a partially constructed map is often referred to as the SLAM (Simultaneous Localization and Mapping) problem. We present an approach to the SLAM problem that is capable of building dense, metric maps of complex domains. In a significant improvement over previous work, our particle filter based algorithm efficiently maintains multiple map hypotheses, yet the run time per iteration scales linearly with the number of particles used. The significance of this algorithmic result is that multiple hypothesis slam with dense maps now has the same computational as particle filter based localization using just a single map. The performance of this method is further improved with the introduction of a novel hierarchical approach to particle filters, wherein the output of several small SLAM processes can be combined in a principled manner using a higher level particle filter. This allows the algorithm to explicitly represent and recover from the drift which is inherent in any sampling based SLAM method.